
// 360 divided by the circumference (25 cm)
#define CONVERSION 76.4
//(3/2 * 1440 / (4 * 3.142))
// was 11.25
#define L_MOTOR1 motorE
#define L_MOTOR2 motorG
#define R_MOTOR1 motorD
#define R_MOTOR2 motorF
#define DELTAPOS 20

//CRATELIFT DEFINITIONS
#define ESP 20
#define CLHOME 917
#define CRATE1 541
#define CRATE2 400
#define SPIN 650
int cltarget = CLHOME;        //lift position
bool cltargetreached = false;// says we have reached target position when true
int delta;
#define SPINFORWARD 255
#define SPINBACKWARD 0
//#define Kp .02

bool stalled = false;
bool firsttimestalled = false;
int prevLMTRpos;
int prevRMTRpos;
int timer1;
int timer2;
int power;
float target;
float error;
int itarget;
int encoderCount;
bool shutdown = false;
//goXCM (40, 75, .02);  // forward (ball hoop)

//goXCM (40,  -75, .02);  // backward (clamp)

	  // Specify which conversion factor to use
#define DIRECTION -1
    // The motors are reversed

/*
goXCM: Command Motor to travel linear distance in inches using closed loop control with proportional gain
distance = distance command in inches
maxPower = maximum power to motor,  normally limited to 75%
           negative ==> backward (crate lift direction)
           positive ==> forward (ball hoop direction)
Kp       = Proportional gain  (eg. .02 for down ramp,  0.07 for flat ground)
*/
void goXCM(int distance, int maxPower, float Kp)

{
    nMotorEncoder[L_MOTOR1] = 0;  //clear the LEGO encoders in motors B and C+
	  nMotorEncoder[R_MOTOR1] = 0;

	  target = distance*CONVERSION;
	  encoderCount = nMotorEncoder[L_MOTOR1];

//	  motor L_MOTOR1 = DIRECTION*power; //turn both motors on at x percent power (L motors -ve = forward)
//	  motor R_MOTOR1 = power;           // R motors -ve = backward
//    motor L_MOTOR2 = DIRECTION*power;
//	  motor R_MOTOR2 = power;

	  itarget = (int) target;
	  while (abs(encoderCount) < abs(itarget))  //Check for reaching target
	  {
	    encoderCount = nMotorEncoder[L_MOTOR1];
	    error = itarget - abs (encoderCount);

	    power =(int) (Kp * error);
	    if (power > maxPower)    // Clip max power to  75
	     power = maxPower;


  	  motor[L_MOTOR1] = DIRECTION*power; //turn both motors on at x percent power
  	  motor[R_MOTOR1] = power;
      motor[L_MOTOR2] = DIRECTION*power;
  	  motor[R_MOTOR2] = power;

	  }
	  motor[L_MOTOR1] = 0;
	  motor[R_MOTOR1] = 0;
	  motor[L_MOTOR2] = 0;
	  motor[R_MOTOR2] = 0;
	  nxtDisplayTextLine(2, "enc %d, %f", encoderCount,target);//displays encoder value on nxt
}
void turnXDg(int distance,int power)
{
	  nMotorEncoder[L_MOTOR1] = 0;  //clear the LEGO encoders in motors B and C+
	  nMotorEncoder[R_MOTOR1] = 0;
    if (power > 0)
	 {
	  nMotorEncoderTarget[L_MOTOR1] = distance * -2.65;
    nMotorEncoderTarget[R_MOTOR1]	= distance * 2.65;
   }
   else
	 {
	  nMotorEncoderTarget[L_MOTOR1] = distance * 2.65;
    nMotorEncoderTarget[R_MOTOR1]	= distance * -2.65;
   }
	  motor[L_MOTOR1] = power; //turn both motors on at 30 percent power
	  motor[R_MOTOR1] = power;
    motor[L_MOTOR2] = power;
	  motor[R_MOTOR2] = power;
    //float fdistance = (float) distance * (2*2.65);// gets the average of the encoder counts faster and the conversion factor multiplied by distance
	  //distance = (int) fdistance;
    //hogCPU();
	 // while ((abs(nMotorEncoder[R_MOTOR1])+abs(nMotorEncoder[L_MOTOR1])) < distance) //while the encoder wheel turns slightly less than 2 revolution
	  //{
	  //}
    //releaseCPU();
}
void onewheelturn(int Rdistance,int Ldistance,int power)
{
	  nMotorEncoder[L_MOTOR1] = 0;  //clear the LEGO encoders in motors B and C+
	  nMotorEncoder[R_MOTOR1] = 0;
    if (power > 0)
	 {
	  nMotorEncoderTarget[L_MOTOR1] = Ldistance * -2.65;
    nMotorEncoderTarget[R_MOTOR1]	= Rdistance * 2.65;
   }
   else
	 {
	  nMotorEncoderTarget[L_MOTOR1] = Ldistance * 2.65;
    nMotorEncoderTarget[R_MOTOR1]	= Rdistance * -2.65;
   }
	if (Ldistance != 0)
 {
    motor[L_MOTOR1] = power; //turn both motors on at 30 percent power
	  motor[L_MOTOR2] = power;
 }
	else
 {
    motor[R_MOTOR1] = power;
	  motor[R_MOTOR2] = power;
 }
}
task cratelift()
{
  int curpos;

  while (true)
  {
    curpos = HTPBreadADC(HTPB, 0, 10); // read position from potentiometer at A0
    delta = curpos - cltarget;
    //nxtDisplayTextLine(6, "Position %d", curpos);
    //nxtDisplayTextLine(7, "Delta %d", delta);

    if(abs(delta) > ESP)
    {
      if (delta > 0)
       servo[servo2] = 158;//255; //move down
      else
        servo[servo2] = 108;//10;//move up 98

      //cltargetreached = false;
     // curpos = HTPBreadADC(HTPB, 0, 10); // read position from potentiometer at A0
     // delta = curpos - cltarget;

    }
    else
    {
      servo[servo2] = 128; // park servo target position reached
      cltargetreached = true;
    }
    EndTimeSlice();
  }
}
void setcratelift(int target)
{
cltargetreached = false;
cltarget = target;
while (!cltargetreached)
{}
return;

}
void clamp (int position)
{
 servo[servo1] = position; // unclamp crate
}

void spin (int position)
{
  servo[servo3] = position;
}

 //task to check for stalled motors
// task stallcheck()
// {
	  // timer1 = time10[T1];
	  // if (timer1>20)  // check for stalled motor every 200 ms
	  // {
	    // if ((abs(motor R_MOTOR1) > 0) || (abs(motor R_MOTOR2) > 0))
	      // if (((abs(nMotorEncoder L_MOTOR1) - prevLMTRpos) < DELTAPOS ) || ((abs(nMotorEncoder R_MOTOR1) - prevRMTRpos) < DELTAPOS))
	        // stalled = true;
	    // if (stalled && !firsttimestalled) firsttimestalled = true;
	    // prevLMTRpos = nMotorEncoder L_MOTOR1;
	    // prevRMTRpos = nMotorEncoder R_MOTOR1;
	    // ClearTimer(T1);
	  // }
//	  check if stalled continuously for 5 s
	  // if (firsttimestalled)
	  // {
	    // ClearTimer(T2);  //start stalled timer
	  // }
	  // if (stalled)
	  // {
	    // timer2 = time10[T2];
	    // if (timer2>10)
	    // {
	       // motor L_MOTOR1 = 0;
	       // motor R_MOTOR1 = 0;
	       // motor L_MOTOR2 = 0;
	       // motor R_MOTOR2 = 0;
      // }
    // }
	  // else
	  // {
	    // firsttimestalled = false;
	    // ClearTimer(T2);  //start stalled timer
	  // }
	  // wait10Msec(1);
// }
